//
// Created by kuang on 2025/9/19.
//

#include "iena_parser.h"

#include <string.h>

/* 内联函数：从缓冲区读取16位值 (大端序) */
static inline uint16_t read_u16_be(const uint8_t *buffer) {
    return ((uint16_t) buffer[0] << 8) | buffer[1];
}

/* 字节数组转换为48位时间戳 */
static uint64_t bytes_to_timestamp(const uint8_t time_bytes[6]) {
    return ((uint64_t) time_bytes[0] << 40) |
           ((uint64_t) time_bytes[1] << 32) |
           ((uint64_t) time_bytes[2] << 24) |
           ((uint64_t) time_bytes[3] << 16) |
           ((uint64_t) time_bytes[4] << 8) |
           ((uint64_t) time_bytes[5]);
}

/**
 * 解析IENA包到结构体
 */
int iena_parse_packet(iena_parsed_packet_t *packet, const uint8_t *data, uint16_t size) {
    if (!packet || !data || size < 16) {
        return -1;
    }

    // 初始化结构体
    memset(packet, 0, sizeof(iena_parsed_packet_t));

    // 解析包头
    packet->iena_key = read_u16_be(&data[0]);
    packet->size = read_u16_be(&data[2]);
    packet->timestamp = bytes_to_timestamp(&data[4]);
    packet->key_status = data[10];
    packet->status = data[11];
    packet->seq_num = read_u16_be(&data[12]);

    // 验证包完整性
    if (packet->size != size || read_u16_be(&data[size - 2]) != 0xDEAD) {
        packet->is_valid = 0;
        packet->parse_error = -2;
        return -2;
    }

    // 确定包类型和参数长度
    if (packet->key_status & 0x80) {
        packet->packet_type = 'P';
        packet->word_length = packet->key_status & 0x07;
    } else if ((packet->key_status & 0x30) == 0x30) {
        packet->packet_type = 'M';
    } else if ((packet->key_status & 0x30) == 0x20) {
        packet->packet_type = 'Q';
    } else if ((packet->key_status & 0x30) == 0x10) {
        packet->packet_type = 'D';
        packet->word_length = packet->key_status & 0x07;
    } else {
        packet->packet_type = 'N';
        packet->word_length = packet->key_status & 0x07;
    }

    // 解析数据体
    const uint8_t *body_start = &data[14];
    uint16_t body_size = size - 16;

    if (packet->packet_type == 'P') {
        // P型包：解析定长参数数组
        uint16_t param_bytes = packet->word_length * 2;
        packet->param_count = body_size / param_bytes;

        for (int i = 0; i < packet->param_count && i < IENA_MAX_PARAMS; i++) {
            const uint8_t *param_data = &body_start[i * param_bytes];

            if (param_bytes == 2) {
                packet->p_params.values_u16[i] = read_u16_be(param_data);
            } else if (param_bytes == 4) {
                packet->p_params.values_u32[i] =
                        ((uint32_t) read_u16_be(param_data) << 16) |
                        read_u16_be(param_data + 2);
            } else {
                // 更长的参数，存储为16位数组
                for (int j = 0; j < param_bytes / 2 && i * 4 + j < IENA_MAX_PARAMS; j++) {
                    packet->p_params.values_u16[i * 4 + j] = read_u16_be(&param_data[j * 2]);
                }
            }
        }
    } else {
        // M/Q/D/N型包：解析参数块
        uint16_t pos = 0;
        int param_idx = 0;

        while (pos < body_size && param_idx < IENA_MAX_PARAMS) {
            if (packet->packet_type == 'M') {
                // M型包：ParamID + Offset + Length + Data
                if (pos + 6 > body_size) break;

                packet->other_params.param_ids[param_idx] = read_u16_be(&body_start[pos]);
                packet->other_params.offsets[param_idx] = read_u16_be(&body_start[pos + 2]);
                packet->other_params.lengths[param_idx] = read_u16_be(&body_start[pos + 4]);
                packet->other_params.data_ptrs[param_idx] = &body_start[pos + 6];

                uint16_t data_len = packet->other_params.lengths[param_idx];
                pos += 6 + data_len;
                if (data_len & 1) pos++; // padding

            } else if (packet->packet_type == 'Q') {
                // Q型包：ParamID + Length + Data
                if (pos + 4 > body_size) break;

                packet->other_params.param_ids[param_idx] = read_u16_be(&body_start[pos]);
                packet->other_params.lengths[param_idx] = read_u16_be(&body_start[pos + 2]);
                packet->other_params.data_ptrs[param_idx] = &body_start[pos + 4];

                uint16_t data_len = packet->other_params.lengths[param_idx];
                pos += 4 + data_len;
                if (data_len & 1) pos++; // padding

            } else if (packet->packet_type == 'D') {
                // D型包：ParamID + Delay + Data
                uint16_t param_bytes = packet->word_length * 2;
                if (pos + 4 + param_bytes > body_size) break;

                packet->other_params.param_ids[param_idx] = read_u16_be(&body_start[pos]);
                packet->other_params.offsets[param_idx] = read_u16_be(&body_start[pos + 2]); // 存储Delay
                packet->other_params.lengths[param_idx] = param_bytes;
                packet->other_params.data_ptrs[param_idx] = &body_start[pos + 4];

                pos += 4 + param_bytes;

            } else { // N型包
                // N型包：ParamID + Data
                uint16_t param_bytes = packet->word_length * 2;
                if (pos + 2 + param_bytes > body_size) break;

                packet->other_params.param_ids[param_idx] = read_u16_be(&body_start[pos]);
                packet->other_params.lengths[param_idx] = param_bytes;
                packet->other_params.data_ptrs[param_idx] = &body_start[pos + 2];

                pos += 2 + param_bytes;
            }

            param_idx++;
        }

        packet->param_count = param_idx;
    }

    packet->is_valid = 1;
    return 0;
}